#ifndef __POSTPROCESSOR_HPP
#define __POSTPROCESSOR_HPP

#include "common_include.hpp"

namespace visionhw
{

    class PostProcessor
    {
    public:
        PostProcessor();
        ~PostProcessor() {}
        void setInputCloud(const CloudType::Ptr cloud_input);
        void process(CloudType &cloud_output, pcl::PointCloud<pcl::FPFHSignature33> &fpfh_cloud);
        inline void setIsPly(bool is_ply) { _is_ply = is_ply; }
        inline auto getFinalCloud() { return *_cloud_in; }
        inline auto getFinalFPFHCloud() { return *_fpfh_cloud; }
        inline void setSaveIntermediatePly(bool flag) { _saveIntermediatePly = flag;}

        inline void setVoxelLeafSize(const double s)
        {
            _voxel.setLeafSize(s, s, s);
            std::cout << "setLeafSize: " << s << "\n";
        }

        inline void setPlaneFilterPara(const int maxiter, const double threshold)
        {
            _plane_seg.setMaxIterations(maxiter);       //设置最大迭代次数
            _plane_seg.setDistanceThreshold(threshold); //设定距离阀值
            std::cout << "setPlaneMaxIterations: " << maxiter << "\n";
            std::cout << "setPlaneDistanceThreshold: " << threshold << "\n";
        }

        inline void setClusterPara(const double tolerance,
                                   const int minsize,
                                   const int maxsize)
        {
            _ec.setClusterTolerance(tolerance); //设置近邻搜索的搜索半径
            _ec.setMinClusterSize(minsize);     //设置一个聚类需要的最少点数目
            _ec.setMaxClusterSize(maxsize);     //设置一个聚类需要的最大点数目为
            std::cout << "setClusterTolerance: " << tolerance << "\n";
            std::cout << "setMinClusterSize: " << minsize << "\n";
            std::cout << "setMaxClusterSize: " << maxsize << "\n";
        }

        inline void setNormalPara(const double radius)
        {
            _normal.setRadiusSearch(radius);
            std::cout << "setNormalRadiusSearch: " << radius << "\n";
        }

        inline void setFPFHPara(const double radius)
        {
            _fpfh.setRadiusSearch(radius);
            std::cout << "setFPFHRadiusSearch: " << radius << "\n";
        }

    private:
        bool _saveIntermediatePly = false;
        void init();
        CloudType::Ptr _cloud_in;
        //保存去除的NAN点的索引
        std::vector<int> _indices_src_rmnan;
        // 下采样
        pcl::VoxelGrid<PointType> _voxel;
        // 去平面
        pcl::ModelCoefficients::Ptr _plane_coefficients;
        pcl::PointIndices::Ptr _plane_inliers;
        pcl::SACSegmentation<PointType> _plane_seg; // 分割对象
        pcl::ExtractIndices<PointType> _extractor;  //点提取对象
        // 聚类
        pcl::search::KdTree<PointType>::Ptr _cluster_tree;
        std::vector<pcl::PointIndices> _cluster_indices;
        pcl::EuclideanClusterExtraction<PointType> _ec;

        pcl::PointIndices::Ptr _tmpp; // 临时的索引指针

        // 法线计算
        pcl::PointCloud<pcl::Normal>::Ptr _cloud_normal;
        pcl::NormalEstimation<PointType, pcl::Normal> _normal;
        pcl::search::KdTree<PointType>::Ptr _normal_tree;

        // FPFH
        pcl::FPFHEstimation<PointType, pcl::Normal, pcl::FPFHSignature33> _fpfh;
        pcl::search::KdTree<PointType>::Ptr _fpfh_tree;
        pcl::PointCloud<pcl::FPFHSignature33>::Ptr _fpfh_cloud;

    private:
        bool _is_ply = false;
    };
}

#endif